#include <gtest/gtest.h>
#include <chrono>
#include <thread>
#include "task/motion_al.h"
#include "safety/io/safeio.h"
#include "axisgroup/axisgroup.h"
#include "axisgroup/axisgroup_robot.h"
#include "axisgroup/robot_mgr.h"

class StartupTest : public ::testing::Test
{
public:
    void SetUp() override {}
    void TearDown() override
    {
        //
        axisgroup::AxisGroupMgr::instance().reset();
    }
};

TEST(StartupTest, iosafe_init) { ASSERT_EQ(safety::SafeIOMgr::instance().init(), 0); }
TEST(StartupTest, motion_init_2_robot)
{
    ASSERT_EQ(safety::SafeIOMgr::instance().init(), 0);
    ASSERT_EQ(0, mot::al::MotionProxy::instance().motion_init());

    EXPECT_NE(nullptr, axisgroup::AxisGroupMgr::instance().get_axisgroup(0).get());
    EXPECT_NE(nullptr, axisgroup::AxisGroupMgr::instance().get_axisgroup(1).get());
    EXPECT_EQ(nullptr, axisgroup::AxisGroupMgr::instance().get_axisgroup(2).get());

    auto ids = axisgroup::AxisGroupMgr::instance().get_axisgroup(0)->get_joint_ids();
    for (size_t i = 0; i < ids.size(); i++) { EXPECT_EQ(i, ids[i]); }
    auto ids2 = axisgroup::AxisGroupMgr::instance().get_axisgroup(1)->get_joint_ids();
    for (size_t i = ids.size(); i < ids.size() + ids2.size(); i++) { EXPECT_EQ(i, ids2[i - ids.size()]); }

    std::this_thread::sleep_for(std::chrono::seconds(10));
}

TEST(StartupTest, all_init)
{
    extern int init_modules();
    ASSERT_EQ(init_modules(), 0);
}